#include "pose_controller/pid.hpp"

PID::PID() {
    this->pid_reset();
}

void PID::pid_reset() {
    this->kd = 0;
    this->ki = 0;
    this->kp = 0;
    this->setspeed = 0;
    this->actspeed = 0;
    this->err = 0;
    this->err_last = 0;
    this->voltage = 0;
    this->integral = 0;
}

float PID::pid_control_position(float err_in, float upper_limit, float upper_speed_limit, float kp, float ki, float kd) {
    this->err = err_in;
    this->kp = kp;
    this->ki = ki;
    this->kd = kd;
    this->integral += this->err;

    if (this->integral > upper_limit) {
        this->integral = upper_limit;
    }
    if (this->integral < -upper_limit) {
        this->integral = -upper_limit;
    }

    this->voltage = this->kp * this->err + this->ki * this->integral + this->kd * (this->err - this->err_last);
    this->err_last = this->err;
    this->actspeed = this->voltage * 1.0;

    if (this->actspeed > upper_speed_limit) {
        this->actspeed = upper_speed_limit;
    }

    if (this->actspeed < -upper_speed_limit) {
        this->actspeed = -upper_speed_limit;
    }

    return this->actspeed;
}
